#include <Eigen/Geometry>
#include <nav_msgs/Odometry.h>
#include <quadrotor_msgs/SO3Command.h>
#include <quadrotor_simulator/Quadrotor.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <uav_utils/geometry_utils.h>

//键盘和规划器输出的控制信息，根据四旋翼无人机控制算法把数据发送，从而作用到无人机的PWM上

//四电机转速
typedef struct _Control
{
  double rpm[4];
} Control;

Control control;
//So3控制命令结构体
typedef struct _Command
{
  float force[3];
  float qx, qy, qz, qw;//目标姿态角四元数
  float kR[3];
  float kOm[3];//目标角速度
  float corrections[3];
  float current_yaw;
  bool  use_external_yaw;
} Command;

static Command     command;

void stateToOdomMsg(const QuadrotorSimulator::Quadrotor::State& state,
                    nav_msgs::Odometry&                         odom);
void quadToImuMsg(const QuadrotorSimulator::Quadrotor& quad,
                  sensor_msgs::Imu&                    imu);

//用于根据命令计算无人机四个电机的转速
    // 无人机系统参数
    //     质量 0.9kg
    //     轴距（电机至机体中心）0.18米
    //     转动惯量 Ixx 0.0046890742, Iyy 0.0069312, Izz 0.010421166
    //     电机升力系数 0.000367717
    //     电机反扭力系数 4.888486266072161e-06
    //     最大转速 11079.03 转每分钟

static Control
getControl(const QuadrotorSimulator::Quadrotor& quad, const Command& cmd)
{
  const double _kf = 0.000367717;//电机升力系数
  const double _km = 4.888486266072161e-06;//电机反扭力系数
  const double kf  = _kf - cmd.corrections[0];
  const double km  = _km / _kf * kf;

  const double          d       = 0.18;//四旋翼轴距
  //const Eigen::Matrix3f J       = quad.getInertia().cast<float>();//四旋翼惯性矩阵，由转动惯量获得
  const float           I[3][3] = { { 0.0046890742, 0, 0 },
                                    { 0, 0.0069312, 0 },
                                    { 0, 0, 0.010421166 } };
  const QuadrotorSimulator::Quadrotor::State state = quad.getState();

  //把无人机的旋转矩阵转化成欧拉角，旋转矩阵由当前无人机姿态获得
  //这里需要把无人机三轴角度数据赋给ypr，待改
  //Eigen::Vector3d ypr = Eigen::Vector3d(yaw_value, pitch_value, roll_value);
  if (cmd.use_external_yaw)
    ypr[0] = cmd.current_yaw;//得到当前无人机的yaw轴角度
  //使用欧拉角计算旋转矩阵
  Eigen::Matrix3d R;
  R = Eigen::AngleAxisd(ypr[0], Eigen::Vector3d::UnitZ()) *
      Eigen::AngleAxisd(ypr[1], Eigen::Vector3d::UnitY()) *
      Eigen::AngleAxisd(ypr[2], Eigen::Vector3d::UnitX());
  float R11 = R(0, 0);
  float R12 = R(0, 1);
  float R13 = R(0, 2);
  float R21 = R(1, 0);
  float R22 = R(1, 1);
  float R23 = R(1, 2);
  float R31 = R(2, 0);
  float R32 = R(2, 1);
  float R33 = R(2, 2);

  //角速度向量的三个分量，需要重新赋值，待改
  float Om1 = state.omega(0);//x
  float Om2 = state.omega(1);//y
  float Om3 = state.omega(2);//z

  //使用四元数计算目标姿态的旋转矩阵
  float Rd11 =
    cmd.qw * cmd.qw + cmd.qx * cmd.qx - cmd.qy * cmd.qy - cmd.qz * cmd.qz;
  float Rd12 = 2 * (cmd.qx * cmd.qy - cmd.qw * cmd.qz);
  float Rd13 = 2 * (cmd.qx * cmd.qz + cmd.qw * cmd.qy);
  float Rd21 = 2 * (cmd.qx * cmd.qy + cmd.qw * cmd.qz);
  float Rd22 =
    cmd.qw * cmd.qw - cmd.qx * cmd.qx + cmd.qy * cmd.qy - cmd.qz * cmd.qz;
  float Rd23 = 2 * (cmd.qy * cmd.qz - cmd.qw * cmd.qx);
  float Rd31 = 2 * (cmd.qx * cmd.qz - cmd.qw * cmd.qy);
  float Rd32 = 2 * (cmd.qy * cmd.qz + cmd.qw * cmd.qx);
  float Rd33 =
    cmd.qw * cmd.qw - cmd.qx * cmd.qx - cmd.qy * cmd.qy + cmd.qz * cmd.qz;

  float Psi = 0.5f * (3.0f - (Rd11 * R11 + Rd21 * R21 + Rd31 * R31 +
                              Rd12 * R12 + Rd22 * R22 + Rd32 * R32 +
                              Rd13 * R13 + Rd23 * R23 + Rd33 * R33));

  float force = 0;
  if (Psi < 1.0f) // Position control stability guaranteed only when Psi < 1
    force = cmd.force[0] * R13 + cmd.force[1] * R23 + cmd.force[2] * R33;

//计算旋转误差
  float eR1 = 0.5f * (R12 * Rd13 - R13 * Rd12 + R22 * Rd23 - R23 * Rd22 +
                      R32 * Rd33 - R33 * Rd32);
  float eR2 = 0.5f * (R13 * Rd11 - R11 * Rd13 - R21 * Rd23 + R23 * Rd21 -
                      R31 * Rd33 + R33 * Rd31);
  float eR3 = 0.5f * (R11 * Rd12 - R12 * Rd11 + R21 * Rd22 - R22 * Rd21 +
                      R31 * Rd32 - R32 * Rd31);


  float eOm1 = Om1;
  float eOm2 = Om2;
  float eOm3 = Om3;
  //计算三轴惯性力矩
  float in1 = Om2 * (I[2][0] * Om1 + I[2][1] * Om2 + I[2][2] * Om3) -
              Om3 * (I[1][0] * Om1 + I[1][1] * Om2 + I[1][2] * Om3);
  float in2 = Om3 * (I[0][0] * Om1 + I[0][1] * Om2 + I[0][2] * Om3) -
              Om1 * (I[2][0] * Om1 + I[2][1] * Om2 + I[2][2] * Om3);
  float in3 = Om1 * (I[1][0] * Om1 + I[1][1] * Om2 + I[1][2] * Om3) -
              Om2 * (I[0][0] * Om1 + I[0][1] * Om2 + I[0][2] * Om3);
  /*
    // Robust Control --------------------------------------------
    float c2       = 0.6;
    float epsilonR = 0.04;
    float deltaR   = 0.1;
    float eA1 = eOm1 + c2 * 1.0/I[0][0] * eR1;
    float eA2 = eOm2 + c2 * 1.0/I[1][1] * eR2;
    float eA3 = eOm3 + c2 * 1.0/I[2][2] * eR3;
    float neA = sqrt(eA1*eA1 + eA2*eA2 + eA3*eA3);
    float muR1 = -deltaR*deltaR * eA1 / (deltaR * neA + epsilonR);
    float muR2 = -deltaR*deltaR * eA2 / (deltaR * neA + epsilonR);
    float muR3 = -deltaR*deltaR * eA3 / (deltaR * neA + epsilonR);
    // Robust Control --------------------------------------------
  */
 //根据误差计算需要的控制力矩
  float M1 = -cmd.kR[0] * eR1 - cmd.kOm[0] * eOm1 + in1; // - I[0][0]*muR1;
  float M2 = -cmd.kR[1] * eR2 - cmd.kOm[1] * eOm2 + in2; // - I[1][1]*muR2;
  float M3 = -cmd.kR[2] * eR3 - cmd.kOm[2] * eOm3 + in3; // - I[2][2]*muR3;

//计算目标四个电机转速
  float w_sq[4];
  w_sq[0] = force / (4 * kf) - M2 / (2 * d * kf) + M3 / (4 * km);
  w_sq[1] = force / (4 * kf) + M2 / (2 * d * kf) + M3 / (4 * km);
  w_sq[2] = force / (4 * kf) + M1 / (2 * d * kf) - M3 / (4 * km);
  w_sq[3] = force / (4 * kf) - M1 / (2 * d * kf) - M3 / (4 * km);

  Control control_return;
  for (int i = 0; i < 4; i++)
  {
    if (w_sq[i] < 0)
      w_sq[i] = 0;

    control_return.rpm[i] = sqrtf(w_sq[i]);
  }
  //返回电机转速结构体
  return control_return;
}

static void
cmd_callback(const quadrotor_msgs::SO3Command::ConstPtr& cmd)
{
  command.force[0]         = cmd->force.x;
  command.force[1]         = cmd->force.y;
  command.force[2]         = cmd->force.z;
  command.qx               = cmd->orientation.x;
  command.qy               = cmd->orientation.y;
  command.qz               = cmd->orientation.z;
  command.qw               = cmd->orientation.w;
  command.kR[0]            = cmd->kR[0];
  command.kR[1]            = cmd->kR[1];
  command.kR[2]            = cmd->kR[2];
  command.kOm[0]           = cmd->kOm[0];
  command.kOm[1]           = cmd->kOm[1];
  command.kOm[2]           = cmd->kOm[2];
  command.corrections[0]   = cmd->aux.kf_correction;
  command.corrections[1]   = cmd->aux.angle_corrections[0];
  command.corrections[2]   = cmd->aux.angle_corrections[1];
  command.current_yaw      = cmd->aux.current_yaw;
  command.use_external_yaw = cmd->aux.use_external_yaw;
}

int
main(int argc, char** argv)
{
  ros::init(argc, argv, "uav_cmdtopwm_node");

  ros::NodeHandle n("~");

  //定义话题发布
  ros::Publisher  pwm_pub  = n.advertise<airsim_ros::RotorPWM>("/airsim_node/drone_1/rotor_pwm_cmd", 100);//发布四个电机PWM值

  //定义话题接收
  ros::Subscriber cmd_sub = n.subscribe("so3_cmd", 100, &cmd_callback, ros::TransportHints().tcpNoDelay());//接收SO3控制命令

  ros::Time next_odom_pub_time = ros::Time::now();
  while (n.ok())
  {
    ros::spinOnce();

    auto last = control;//上次四旋翼无人机电机值
    control   = getControl(quad, command);//计算得到本次四旋翼无人机电机值
    for (int i = 0; i < 4; ++i)
    {
      if (std::isnan(control.rpm[i]))
        control.rpm[i] = last.rpm[i];
    }

    //pwm控制信息发送
    airsim_ros::RotorPWM pwm_cmd;
    pwm_cmd.rotorPWM0 = control.rpm[0];
    pwm_cmd.rotorPWM1 = control.rpm[1];
    pwm_cmd.rotorPWM2 = control.rpm[2];
    pwm_cmd.rotorPWM3 = control.rpm[3];
    pwm_pub.publish(pwm_cmd);//发布控制信息
  }

  return 0;
}
